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This  paper  presents  the  design  of  the  GNC  system  of  a  ground  test-bed  for  spacecraft 
rendezvous  and  docking  experiments.  The  test-bed  is  developed  within  the  STEPS  project 
(Systems  and  Technologies  for  Space  Exploration).  The  facility  consists  of  a  flat  floor  and 
two  scaled  vehicles,  one  active  chaser  and  one  “semi-active”  target.  Rendezvous  and 
docking  maneuvers  are  performed  floating  on  the  plane  with  pierced  plates  as  lifting 
systems.  The  system  is  designed  to  work  both  with  inertial  and  non-inertial  reference 
frame,  receiving  signals  from  navigation  sensors  as:  accelerometers,  gyroscopes, 
laser  meter,  radio  finder  and  video  camera,  and  combining  them  with  a  digital  filter. 
A  Proportional-Integrative-Derivative  control  law  and  Pulse  Width  Modulators  are  used 
to  command  the  cold  gas  thrusters  of  the  chaser,  and  to  follow  an  assigned  trajectory 
with  its  specified  velocity  profile.  The  design  and  development  of  the  guidance, 
navigation  and  control  system  and  its  architecture — including  the  software  algorithms — 
are  detailed  in  the  paper,  presenting  a  performance  analysis  based  on  a  simulated 
environment.  A  complete  description  of  the  integrated  subsystems  is  also  presented. 

©  2013  IAA.  Published  by  Elsevier  Ltd.  All  rights  reserved. 


1.  Introduction 

The  development  of  Rendezvous  and  Docking  space 
systems  [1]  requires  an  experimental  analysis  [2  in 
order  to  test  and  validate  the  docking  technologies,  the 
control  systems,  the  navigation  methods,  the  proximity 
operations  [3],  the  model  algorithms  and  the  numerical 
simulations.  Nevertheless,  costs  and  time  prevents  the 
possibility  of  the  on-orbit  experimental  approach  [4],  so 
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the  availability  of  ground-based  test  facilities  that  can 
reproduce  the  conditions  of  a  RV&D  maneuver  is  strongly 
desirable  [5].  The  scenario,  regarding  this  type  of  test¬ 
beds,  involves  different  research  activities  and  develop¬ 
ments,  focused  on  the  implementation,  validation  and 
verification  of  the  guidance,  navigation  and  control  algo¬ 
rithms.  Some  examples  are  shortly  presented  hereafter, 
without  claiming  to  be  exhaustive. 

At  the  US  Naval  Postgraduate  School  in  Monterey, 
three  generations  of  in-plane  orbital  RV&D  test-beds  have 
been  realized  [6],  the  last  of  which  has  demonstrated 
both  the  validity  of  navigation  filters  with  GPS,  magnet¬ 
ometer  and  gyroscope  sensors  and  collision  avoidance 
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algorithms.  The  MIT  SPHERES  [7]  (Synchronized  Position 
Hold  Engage  and  Reorient  Experimental  Satellite)  project 
uses  the  NASA’s  2D  laboratory  platform  and  the  Interna¬ 
tional  Space  Station  as  test-bed  to  validate  control 
algorithms,  autonomous  docking  [8]  and  high-risk  mea¬ 
surements  between  three  cooperative  vehicles.  The  EPOS 
(European  Proximity  Operation  Simulator)  facility,  recently 
modernized,  is  another  example  of  hardware-in-the-loop 
simulator  offering  the  capability  of  real-time,  real-size 
simulation,  combined  with  realistic  simulation  of  Sun  in 
the  sensor’s  field  of  view.  Verification  of  GNC  algorithms 
and  data  processing  is  possible  thanks  to  the  Motion 
System  Control  linked  to  the  simulator  [9].  Another 
relevant  example  is  the  Technion’s  Distributed  Space 
Systems  Laboratory,  developed  for  formation  flight 
ground  experiments  [10].  Finally,  at  NASA  Jet  Propulsion 
Laboratory,  it  was  demonstrated  the  value  of  a  formation 
flight  algorithm  named  FAST  (Formation  Algorithms  and 
Simulation  Test-bed),  conceived  to  bring  out  issues  in 
data  fusion,  inter-spacecraft  communication  and  sensing. 
The  aim  of  this  algorithm  is  to  find,  through  the  use  of 
reaction  wheels,  gyros,  star  trackers  and  a  ground  hard¬ 
ware  formation  test-bed,  Earth-like  planets  orbiting  dis¬ 
tant  stars  [11]. 


2.  Present  work 

The  present  research  is  carried  out  in  the  frame  of 
STEPS  program-Systems  and  Technologies  for  Space 
Exploration — a  research  project  co-financed  by  Regione 
Piemonte  (Piedmont  Region)  within  the  P.O.R.-F.E.S.R. 
2007-2013  EC  program.  One  of  the  aims  of  this  program 
is  to  reproduce  on  the  ground  the  phases  of  a  spacecraft 
RV&D  mission  starting  from  the  phasing  up  to  the  dock¬ 
ing.  Considering  the  vehicles  operating  in  an  orbital 
scenario  (such  as  those  involving  the  International  Space 
Station)  and  the  same  time  scale,  linear  velocities  and 
distances  are  scaled.  Then  the  test-bed  represents  the 
scaled  condition,  with  a  scale  factor  of  1/18,  of  two 
vehicles  that  are  getting  out  of  the  inertial  navigation 
and  entering  in  the  far  range  rendezvous  phase,  charac¬ 
terized  by  a  relative  navigation  system.  Once  the  chaser 
probe  is  inside  the  target  cone,  the  docking  phase  starts 
allowing  the  docking  mechanism  final  latching. 

The  developed  guidance,  navigation  and  control  algo¬ 
rithms  shall  support  the  vehicles  (3-DOFs)  docking  with 
the  required  precision.  The  GNC  shall  also  interface  with 
the  MM  A  (Mission  Management  Agent)  software  (that 
detects  possible  failures  or  mission  violation),  the  sensors 
and  the  actuators.  Only  the  active  vehicle,  the  chaser,  is 
linked  with  the  ground  Control  Center  (CC)  in  order  to 
exchange  wireless  telemetry  and  command  data.  The 
passive  vehicle,  the  target,  does  not  communicate  neither 
with  the  Control  Center  nor  with  the  chaser,  having  a 
predefined  trajectory. 

Main  contributions  of  this  work  to  the  state  of  the 
art  are: 

•  Development  of  a  technological  area  (laboratory)  with 

hardware  equipment  (docking  mechanism,  navigation 


sensors,  communication  layer)  and  software  functions 
(GNC,  mission  planning  and  graphical  user  interfacing); 

•  Development  and  evaluation  of  various  types  of  naviga¬ 
tion  filters  able  to  manage  information  from  multiple 
sensors  simultaneously,  allowing  a  redundant  sensing 
configuration,  including  the  handling  of  failures; 

The  paper  is  organized  as  follows:  the  first  section 
presents  the  spacecraft  simulator  used  for  the  experi¬ 
ments.  The  second  section  outlines  the  GNC  architecture, 
including  the  navigation  filter  in  its  “information”  form, 
the  control  logics  that  actuate  thrusters  for  maneuvering 
and  the  guidance  algorithm.  The  third  section  describes 
the  “plant  subsystem”  that  is:  the  reference  equations  of 
motion,  the  model  of  sensors  and  thrusters  including 
actuation  errors.  Last  section  shows  the  simulation  results 
in  order  to  demonstrate  the  potential  controllability  of  the 
vehicles  and  to  show  the  effect  of  a  sensor  failure  on  the 
overall  performance. 

3.  The  experimental  rig 

The  experimental  rig  allows  for  the  verification  of 
guidance,  navigation  and  control  algorithms  in  an  in¬ 
plane  close  proximity  flight  condition.  The  vehicles  float 
using  a  thin  air  film  of  60  pm  over  a  very  flat  floor, 
reproducing  a  nearly  frictionless  environment  in  planar 
dynamic  with  3-DOFs,  i.e.  two  degrees  of  freedom  for 
translation  and  one  for  rotation.  The  docking  mechanism 
function  has  been  preliminarily  tested;  the  aim  of  the 
docking  mechanism  is  connecting  the  two  vehicles  by 
means  of  a  probe  and  three  latches,  docking  the  cone, 
hosted  by  the  target  vehicle  (Fig.  1). 

The  target  vehicle  is  supposed  to  keep  on  moving  on  a 
pre-definite  trajectory  (known  by  the  chaser)  while  the 
chaser  vehicle  has  full  maneuver  capabilities.  Vehicles 
motion  is  controlled  by  14  cold  gas  actuators  with  3 
different  levels  of  thrust. 

The  vehicles  (Fig.  2)  consist  of  one  cylindrical  air  tank 
placed  vertically  on  a  lifting  plate  and  two  decks.  On  the 
decks  take  place  all  the  electric  and  electronic  devices 
together  with  the  thrusters,  the  pneumatic  system  and 
the  docking  mechanism.  There  are  three  different  refer¬ 
ence  frames:  a  fixed  reference  coinciding  with  the  floor 
(Omer :  xmer,yiner),  a  body  reference  for  each  vehicle  ( Ochaser : 
xChaser>y chaser)  and  a  relative  reference  frame  centered  in 
the  target  center  of  gravity  (Oref :  xre/,yre/)  (Fig.  3).  The 
docking  mechanism  and  the  thrusters  are  mounted  on  the 
middle  deck,  which  is  closer  to  the  vehicle  center  of 
gravity,  avoiding  the  generation  of  torque  disturbances 
along  x-body  and  y-body  reference  frame. 

The  chaser  is  equipped  with  4  sensors:  radio  finders, 
laser  position  finders,  video  camera  (providing  position 
and  attitude  measurements  with  respect  to  the  target 
vehicle)  and  an  Inertial  Measurement  Unit  (IMU)  provid¬ 
ing  linear  accelerations  and  angular  velocity  measure¬ 
ments  with  respect  to  the  chaser-body  reference  frame. 
Body  acceleration  and  angular  velocity  are  then  bias- 
compensated  and  transported  in  the  inertial  reference 
frame  ( Oiner :  xiner,yiner).  The  laser  position  finder  can  per¬ 
form  measurements  with  respect  to  the  inertial  reference 
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Structures:  2  decks  of  sandwich  panel 
and  a  lifting  plate  of  stiffened  stainless 
steel, 
m  =  55  kg 
h  =  950  mm 
0  =  600  mm 


Chaser 


Ground 

Station 


Docking  Vlechanism 


Target 


Fig.  1.  The  experimental  rig:  the  chaser  (right)  and  the  target  (left). 


Fig.  2.  The  RV&D  experimental  rig:  chaser  and  target  vehicles. 


frame  and  it’s  used  also  to  correct  IMU’s  drift.  The  sensor 
application  range  is  outlined  in  Fig.  4,  where  also  the 
required  precisions  are  indicated,  as  a  function  of  relative 
distance.  These  precisions  have  been  calculated  applying 
the  scaling  factor  approach  defined  previously.  Up  to  2  m 
of  relative  distance  the  absolute  navigation  is  used. 
During  relative  navigation  (below  2  m)  the  linear  preci¬ 
sion  required  is  5.5  mm  and  the  angular  one  is  1°. 

Unlike  the  chaser,  the  target  doesn’t  need  a  relative 
navigation  system;  it  carries  the  transmitter  of  the  radio 


finder  system  and  the  optical  marker  for  the  camera, 
required  by  the  chaser  as  reference  for  its  navigation 
sensors.  The  target  moves  with  respect  to  the  inertial  refe¬ 
rence  frame  system  by  using  only  an  IMU  and  the  laser 
position  finders. 

The  GNC  software  has  been  developed  in  Matlab- 
Simulink®  environment.  Executable  C-files  are  then  cre¬ 
ated  via  Simulink  Coder®  tool  (for  “nidll.tlc”  target)  and 
uploaded  on  the  processor  of  chaser  and  target  vehicles. 
The  GNC  system  communicates  with  other  two  software 
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tools:  the  Control  Center  (CC)  and  the  MMA  supervisor 
module.  The  CC,  running  on  the  remote  control  station 
PCs,  collects  all  the  information  provided  by  the  entire 
system  via  wireless  communication  (i.e.  expected  and 
estimated  states,  thruster  command,  mission  phase, 
recovery  state,  feed  line  pressure,  lifting  and  thrusters 
air  flow),  making  them  available  for  the  users  with  a 
graphical  interface  developed  in  LabView®  environment. 
The  user  can  define  a  new  mission  through  the  CC,  which 
sends  to  chaser  GNC  the  parameters  initialization.  The 


MMA  software  takes  care  of  the  planning,  execution  of 
the  maneuvers  and  data  exchange  with  the  CC  (failure 
detection,  isolation/identification  and  recovery  modules). 
This  module  interacts  continuously  with  the  GNC  system: 
every  time  an  alarm  is  detected,  a  command  of  station 
keeping  is  sent  to  the  GNC  as  recovery  action.  The  MMA 
compares  the  estimated  state  and  the  expected  state  from 
GNC,  checking  that  the  maximum  allowable  errors  are  not 
exceeded.  The  MMA  also  compares  the  information  from 
the  sensors  in  order  to  verify  their  consistency.  In  both 
cases  an  alarm  signal  is  detected,  then  a  management  by 
exception  logics  is  implemented. 

Figs.  5  and  6  give  an  overall  view  of  the  complete 
configuration  of  the  chaser  and  target  subsystems.  The 
Table  1  summarizes  the  main  electronic  hardware  and 
drivers  of  the  vehicles. 

4.  The  GNC  algorithm 

The  GNC  algorithm  has  to  translate  the  instructions 
defined  in  a  Mission  Plan  Matrix  (MPM)  into  vehicles’  desired 
position  and  attitude  (Guidance),  to  estimate  the  vehicles’ 
position  according  to  sensors  information  (Navigation)  and 
issue  the  opening/closing  command  to  the  thrusters  (Con¬ 
trol).  Currently  the  algorithms  of  the  chaser’s  GNC  allow  to: 

1 )  Move  chaser  vehicle  along  the  path  predefined  in  the 

Mission  Plan  Matrix,  calculating  its  own  position  with 

respect  to  both  the  target  and  the  ring  reference  frame. 
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Fig.  4.  Sensor  operative  range  and  precision  requirements. 
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Fig.  5.  Chaser  equipment. 
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Fig.  6.  Target  equipment. 
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Table  1 

Electronics  hardware  and  sensors  description. 


Part’s  name 
description 

Details 

Description 

Power 

consumption 

(W) 

Main  Control 
Unit 

Peripheral 

Devices 

Embedded  FPGA  type  VIRTEX-5  LX30 

Embedded  block  RAM:  1,152  Kbits.  Processor: 
MPC5200  Free  scale  400  MHz  Lab  View®  Real 
Time  Operating  System. 

WI-FI  Client  IEEE802.11.i  full  compatibility. 
Supported  protocols:  ARP,  UDP,  TCP,  Telnet,  ICMP, 
SNMP,  DHCP,  HTTP,  SMTP.  32  Channels  Digital 
I/O,  TTL  Level  with  independent  channels  I/O 
configuration.  32  Channels  Analog  input, 

±  200  mV  to  +  10  V  input  range,  16  Bit 
resolution. 

8  W 

7  W 

WI-FI  Router 

Installed  on  the  chaser,  allows  to  communicate 
with  Control  Centre 

IEEE802.11.i  full  compatibility.  TRIP  encryption. 
64/128  bit  WEP  encoding. 

3  W 

Laser  Position 

Sensor 

Triangulation  mode  distance  acquisition  referenced 
to  a  test-bed  peripheral  ring  and  distance  acquisition 
referenced  to  the  target  mirrors 

Measurement  rate:  0.02  s  Distance  measurement 
calibration  curve  for  Accuracy:  +  0.2%, 
Repeatability:  0.5%  Measurements  independent 
from  surface  characteristics  or  colors 

3  x  1,5  W 

Radio  Finder 

Sensor 

Based  on  a  triaxial  isotropic  sensors  array  (installed  on  the 
chaser  vehicle)  able  to  receive  and  discriminate  an  encoded 
emission  from  an  isotropic  source  (installed  on  the  target 
vehicle). 

Angular  accuracy:  ±  2.5  °  Angular  position 
repeatability:  +  2°  Position  calibration  functions 
Digital  data  output  Minimum  dynamic 
bandwidth:  100  Hz  Signal/noise  ratio:  >  30  dB 

6  W 

Video  Camera 

Sensor 

Installed  on  the  chaser,  recognize  the  marker  on  target 
Connected  to  the  acquisition  system  through  Wireless  LAN 
data  link. 

DLINK  DCS-6620G 

8  W 

IMU  sensor 

Installed  on  the  chaser  and  the  target,  IMU  is  updated  in 
position  from  the  laser  sensors  absolute  position  data. 

Output  modes:  100  Hz  for  Euler  angles  MEMS 
Sensors  suite:  accelerometers,  gyroscope, 
magnetometers,  temperature  0.1%  minimum 
Repeatability:  0.20°  Accuracy:  +  0.5°  typical  for 
static  test  conditions;  +2.0°  typical  for  dynamic 
(cyclic)  test  conditions  and  for  arbitrary 
orientation  angles  (no  drift) 

1  W 

Power  and 
Emergency 
Management 
Unit 

Controls  the  system  power  distribution  to  the 
chaser  and  target  vehicles. 

Interface  the  Main  Control  Unit  to  the  on-board 
devices  (Thrusters,  pneumatic  valves, 
transducers)  Generate,  following  an  Operator 
action,  a  protected  RF  command  able  to  shut¬ 
down  the  chaser  or  the  target  operations. 

2  W 
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Fig.  7.  GNC  architecture. 


2)  Receive  information  from  MMA  about  failure  of  sen¬ 
sors  and  about  of  new  mission  plan  as  a  consequence 
of  a  corrective  action. 


3)  Send  to  the  Control  Center  the  telemetry  information 
and  receive  back  data  about  the  images  gathered  by 
the  camera. 
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The  control  center  receives  the  image  via  wireless  link, 
it  performs  the  image  processing  and  it  re-sends  position 
and  attitude  to  the  chaser.  The  control  center  represents  a 
remote  auxiliary  computation  unit. 


The  architecture  of  GNC  subsystems  is  represented 
in  Fig.  7. 

4.2.  The  navigation  algorithm 


4A.  The  guidance  algorithm 


The  guidance  algorithm  is  implemented  to  maneuver 
the  chaser  along  the  selected  trajectory  that  can  be 
defined  in  terms  of  waypoints  (Xj,y,  or  6j)  and  velocity 
(Vdior  6j)  listed  in  the  Mission  Plan  Matrix  (Fig.  7): 


MissionPlanMatrix  = 


Xi-Xi- 1 
dj-dj- 1 

Tstk 


yi 

y~yi- 1 


e 


vdi 

vdi 

o 

0 

0 


(1) 


The  first  column  indicates  the  type  of  maneuver 
(i  stands  for  translational  or  station  keeping  maneuver j 
stands  for  attitude  maneuver).  The  second  and  third 
columns  indicate  the  waypoint  coordinates  to  be  reached, 
also  expressed  as  function  of  preceding  waypoints 
(Xj— Xi_! ,yi— y i-i,0 j— 0/_i).  In  case  of  a  station  keeping 
maneuver,  the  second  column  Tstk  indicates  the  duration, 
in  seconds,  of  the  maneuver  itself.  The  desired  state  in 
terms  of  velocity  and  position  profiles  reflects  the  typical 
sequence  acceleration-drift-deceleration  both  in  transla¬ 
tional  and  angular  maneuvers  between  two  waypoints. 
So  the  guidance  algorithm  calculates  the  time  of  each 
translational  maneuver: 


AT/in  = 


AS 

(1  ~k)Vdi 


(2) 


where  AS  =  y/x?+> f  is  the  total  displacement  read  from 
MPM  of  the  i-th  maneuver,  Vdi  is  the  drift  velocity  read 
from  third  column  of  MPM  and  k  is  the  percent  duration 
of  the  transient  at  constant  acceleration  with  respect  to 
the  total  maneuver  time  between  two  waypoints.  In  this 
way  the  velocity  profile  is  a  ramp  signal  with  a  constant 
acceleration  equal  to: 


Vdi  4(1  -k) 
1  kATim  kAS 


(3) 


The  present  simulations  are  obtained  assuming  k  =  0.1. 
Similarly  the  attitude  maneuver  has  a  duration  equal  to: 


ATatt 


h 

(1  -k)0j 


(4) 


that  leads  to  a  constant  angular  acceleration: 

,  6j  gjO-fc) 

>  kATaa  k6j 


(5) 


In  this  way  a  sequence  of  linear  and  angular  maneu¬ 
vers,  followed  with  constant  acceleration  are  created  as 
function  of  time.  The  desired  state  is  now  compared  with 
the  estimated  state,  computed  by  the  navigation  algo¬ 
rithm,  and  the  difference  is  rotated  in  the  body  reference 
frame.  This  error  is  the  input  for  the  control  algorithm. 


The  measurements  from  the  different  sensors  are 
fused  in  a  single  estimate  for  position  and  velocity  by  an 
Information  Kalman  Filter  (IKF)  [12].  The  IKF  performs  a 
data  fusion  process  between  all  the  measurements  com¬ 
ing  from  the  proper  sensors.  This  particular  form  of  the 
filter  was  chosen  due  to  its  simple  additive  nature  of  the 
update  stage  resulting  computationally  simpler  than  the 
equation  of  classic  Kalman  filter  [13],  at  the  cost  of 
increased  complexity  in  prediction. 

The  measurements  available  from  the  chaser’s  i-  sensor 
are  position  and  attitude,  both  in  relative  and  absolute 
reference  frame  and  they  are  represented  by  the  vector 
z2(/<)  =  [xyO].  Thanks  to  the  presence  of  the  IMU,  there  are 
two  possible  choices  of  the  control  vector  u  inside  the  filter: 
the  first  equals  u  to  the  acceleration  and  angular  velocity 
from  the  IMU;  the  second  equals  u  to  the  forces  and  torque 
resulting  from  the  ON-OFF  thrusters  output  signal.  The  first 
case  prevents  the  use  of  INS  integration’s  results  inside  the 
measurement  vector  z,(/<),  so  during  absolute  navigation  if  a 
failure  of  the  laser  meter  occurs  the  mission  should  be 
interrupted,  as  the  laser  is  the  only  measurement  available. 
Instead  the  second  case  allows  to  use  the  INS  integration 
inside  the  measurement  vector  zf(k),  since  the  control  vector 
u  is  a  derivation  of  command  thruster  actuation.  So  if  the 
laser  sensor  fails  during  absolute  navigation  the  redundancy 
is  guaranteed  by  the  INS.  Since  the  accuracy  of  laser  is  2  mm 
and  the  drift  of  INS  navigation  errors  is  the  order  of  5  cm 
every  40  s,  the  second  navigation’s  architecture  is  chosen. 
Therefore  the  data  from  INS  navigation  were  considered 
inside  the  measurement  vector  containing  a  proper  noise 
and  drift  rate  (directly  reset  by  laser  measurements). 
The  time  of  INS  reset  is  dictated  by  the  requirement 
of  accuracy  on  position  and  attitude. 

The  IKF  equations  of  prediction  stage  for  a  system  with 
discrete-time  dynamic  model  are: 

x  (/<,/<- 1)  =  ^x(/c-l,/c-l)+B/cu(/<)  (6) 

p(k,k-i)=^p(k-i,k-i)4+sT^c[  (7) 

where  x  =  [x,y,0,x,y,0]  is  the  state  vector  of  the  system, 
<P  kis  the  state  transition  matrix,  Bk is  the  input  matrix  and 
u  the  control  vector,  Gk  =  &k  Bk  is  the  process  noise 
coupling  matrix.  The  error  covariance  matrix  Pis  com¬ 
puted  taking  into  account^  the  system  noise  represented 
by  the  covariance  matrix  Qk.  The  measurements  vector  is: 

zi(k)  =  Trix(k,k-\)+bi(k,k-\)  i  =  l,....N  (8) 

where  N  is  the  number  of  sensors,  H,  represents  the 
measurement  matrix  and  h{  is  the  zero-mean  white 
Gaussian  (ZMWG)  noise  affecting  the  measurementwith 
an  assumed  known  covariance  matrix  P*  (k)  =  E[bi(k)bJ (/<)]. 

This  last  matrix  changes  its  values  depending  on: 

1)  The  relative  distance  between  vehicles,  in  order  to 

switch  from  absolute  to  relative  measurements. 
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2)  The  alarm  signal  from  MMA  about  a  detected  sensor 
failure.  If  the  i-sensor  is  OFF  or  broken  b,(/<:)  =  100  so 
that  the  information  will  not  be  reliable. 

Given  the  Eqs.  (6)  and  (8)  it  is  possible  to  calculate  the 
state  estimation.  The  estimation  stage  is  computed  by: 

N  _  _ 

x(k,k)  =  x(k,k- 1)+  ^2  Ki  [A (/<)  -  H,  x(/<,  /<  - 1 )]  i  =  1  ,....N  (9) 

where  the  state  x  is  the  state  prediction  and  K{  is  the 
Kalman  gain  matrix  which  realizes  the  balancing  between 
the  sensors  measurements  and  the  dynamic  state  predic¬ 
tions.  The  error  covariance  P(k)  matrix  is  updated  taking 


A  ■  0.125  N  C  ■  0.125  N 

B  0.375  N  D  |  0.75  N 


Fig.  8.  Thrusters  grouping. 


into  account  the  Kalman  gains  calculated  separately  for 
each  sensor. 


P  0 Kk)  =  P  (/<,/<-!)+ Rt  Hf  i  =  1.....N 


(10) 


Ki  =  P(ktk-l)HiTRi  1  2  =  1.....N  (11) 

The  calculation  of  the  state  begins  entering  the  prior 
estimate,  x(0)  and  P( 0),  then  the  Kalman  gain  matrix  is 
computed  with  Eq.  (11)  and  the  prior  estimate  is  updated 
with  measurements  by  using  Eqs.  (9)  and  (10)  This  is  the 
filtered  state  estimate  to  be  used  by  the  control  system  at 
time  /<,  and  the  new  prior  estimate  is  given  by  the 
prediction  of  system  dynamics  for  time  /c+1.  Once  the 
state  is  estimated,  it  is  compared  with  the  expected  state 
coming  out  from  Guidance  algorithms. 

4.3.  The  control  algorithm 

The  Control  algorithm  receives  as  input  (with  a  fre¬ 
quency  of  2  FIz)  the  difference  between  desired  and  esti¬ 
mated  state  and  then  it  gives  as  output  (with  a  frequency  of 
2  Hz)  the  time  opening  command  for  the  14  electro  valves. 
Then  the  PWM  manages  these  opening  timing  at  a  fre¬ 
quency  of  20  Hz.  This  algorithm  works  considering  the  body 
reference  frame  (CoG, chaser  :  xbody,ybody,zbody ),  where  the 
second  axis  is  oriented  along  the  docking  mechanism,  zbody 
coincides  with  the  axis  of  symmetry  of  the  vehicles  and  it  is 
oriented  upwards,  xbody  completes  the  right-handed  refer¬ 
ence  frame.  In  the  control  algorithm,  the  14  cold  thrusters  of 
the  propulsion  system,  are  grouped  into  three  separate  sets 
as  shown  in  Fig.  8: 


Table  2 

Possible  thruster  configuration. 


Level 

Thrust 

IN] 

Configuration 

Level 

Torque 

[Nm] 

Configuration 

Fi 

0.25 

A  or  C 

M! 

0.0685 

A 

f2 

0.75 

A  and  C 

m2 

0.1912 

B 

f3 

1.00 

A+B  and 

C  +  D 

m3 

0.529 

A+B 

•  Group  X:  The  thrusters  C  and  D  belong  to  this  group 
and  they  allow  translational  motion  along  X-axis. 

•  Group  Yl:  The  thrusters  A  and  B  with  x-negative 
coordinates  belong  to  this  group. 

•  Group  Y2:  The  thrusters  A  and  B  with  x-positive 
coordinate  belong  to  this  group. 

The  Y-groups  allow  the  translational  and  rotational 
motion  if  properly  switched. 


X 


Yl 

Y2 


[7X2]  Matrix 
ForThruster  Mapping 
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Fig.  9.  Thrusters  mapping. 
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Fig.  10.  (A)  On-time  during  deceleration  phase  and  (B)  corresponding  ON/OFF  command. 
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Fig.  11.  The  Simulink®  GNC  screenshot. 
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Each  group  has  three  levels  of  thrust  and  the  selected 
level  is  automatically  set  depending  on  the  impulse 
required  to  minimize  the  error  along  xbody,ybody  directions 
and  6body  attitude  (difference  between  estimated  and 
desired  state).  The  data  in  Table  2  associate  the  three 
levels  of  thrust  for  the  14  cold  gas  thruster. 

The  impulse  required  is  calculated  using  a  PID  control 
logic  for  each  of  the  three  DOFs.  The  control  logic  is 
activated  only  if  the  error  on  position  and/or  attitude  and/ 
or  velocity  is  outside  a  dead  band:  the  more  stringent  dead 
band  is  set  during  the  last  1  m  range,  for  the  position  is 
+  5.5  mm,  and  that  of  attitude  is  ±1°.  If  the  error  between 
desired  and  estimated  state  remains  within  these  limits  no 
command  is  produced.  For  example,  along  the  y-axis  (coin¬ 
ciding  with  Y-group)  the  required  impulse  is  equally  dis¬ 
tributed  between  Y1  and  Y2  groups  and  its  value  is: 

|y(0  =  ^control[Kp(y nav(fy—y gui(ty)  Ko(y nav(fy—y gui(ty) 

+«i  ( /  y„av(t)-ygui(t ))] 


In  the  case  of  x-axis  the  pulse  is  assigned  to  the 
X-group.  The  three  gains  I<pJ<dJ<iare  calculated  using 
the  Ziegler-Nichols  tuning  method.  In  the  equation 
At controi  =  0.5 s  is  the  time  interval  at  which  the  control 
generates  the  parameter  ztbr,  ranging  from  0  to  10  (where 
10  represents  the  saturation  of  signal),  defined  as: 


Tthr  — 


In  1 

Fyi  max  A  tpwM 


(12) 


where  IY\  =Iy(t)/2  is  the  required  pulse  for  the  Y1  group, 
Fyimax  is  the  maximum  thrust  deliverable  for  that  level, 
among  those  listed  in  Table  2  and  A tPWM  =  0.05s  is  the  time 
interval  at  which  a  PWM  control  generates  the  ON/OFF 
commands  for  the  electro  valves.  Thus  zthr  is  initially 
calculated  for  X,  Y1  and  Y2  groups,  and  then  referred  to 
each  thruster  through  a  mapping  matrix,  described  in  Fig.  9. 

At  this  point  the  PWM  receives  as  input  a  [7x2] 
matrix  containing  the  on-time  from  0  to  10  of  each 
thruster  and  compares  it  to  a  repeating  sequence  ramp. 
From  this  comparison  the  ON/OFF  Boolean  command 
signal  for  the  single  thruster  (with  a  rate  of  20  Hz)  is 
calculated. 

The  evolution  of  zthr  for  the  thruster  no  6  during  a 
maneuver  and  the  corresponding  Boolean  command  pro¬ 
duced  by  the  PWM  are  plotted  in  Fig.  10.  If  Tthr  =  S  the 
PWM  output  is  a  ON-command  of  0.025  s.  The  saturation  is 
reached  during  acceleration  and  deceleration  transients. 


5.  The  plant 

The  plant  is  represented  through  the  modeling  of  test¬ 
bed  dynamics  in  Matlab-Simulink®  environment,  in 
which  it  is  possible  to  test  the  behavior  of  the  above- 
mentioned  GNC  system  without  the  hardware  in  the  loop. 

The  plant  is  composed  of  three  main  systems  (Fig.  11) 
that  emulate  the  real  test-bed  functions  and  so  directly  or 
indirectly  connect  the  GNC  system  to: 

•  Actuator  Model:  it  takes  into  account  the  ON/OFF 
thruster  switching  time  and  the  uncertainty  of  the 
thrusters  position  on  the  vehicle. 


•  Dynamics  Model:  it’s  the  3  DOFs  dynamic  model 
of  two  rigid  bodies  with  constant  mass  in  relative 
motion. 


Inertial  Test-bed 

frame  Floor  plane 


Moving  Centered  on 

frame  CoG  Target 


Principal 
axes  of 
inertia 


Centered  on 
CoG  Chaser 
and  Target 


Fig.  13.  Orbital  and  test-bed  reference  frames. 
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•  Sensor  Model:  it  implements  the  disturbances  and  the 
errors  of  the  sensors. 


5.1.  The  actuator  model 


The  14  thrusters  are  ideally  fixed  in  their  positions  and 
perfectly  distributed  with  respect  to  the  center  of  mass,  so 
there  are  no  residual  torque  error  due  to  misalignment. 
The  Actuator  Model  allows  calculation  of  force  and  torque 
applied  to  the  vehicles  that  will  be  used  by  both  the  IKF, 
as  control  parameter  u,  and  the  Dynamic  Model  to 
integrate  the  motion  and  attitude  equations. 

The  ON/OFF  thrusters  switching  time  is  modeled 
applying  a  transfer  function  of  the  first  order  to  the 
PWM  output  signal  of  each  thruster: 


F(s)  = 


1 

fcs+l 


(13) 


since  the  ON  commutation  time  is  equal  to  8  ms,  the 
coefficient  of  S  is  k  =  0.008 /n  where  k  indicates  that  98% 
of  the  thrust  is  reached  after  8  ms.  So  the  transfer  function 
is  F(s)  =  1  /0.002s  +  l.  In  Fig.  12  the  ideal  ON  command  is 
compared  with  the  real  one  [14]. 


5.2.  The  Dynamics  model 

Since  the  chaser  is  meant  to  work  both  with  the 
inertial  and  the  relative  reference  frame,  the  dynamic 
model  implemented  computes  the  motion  and  attitude  of 
the  chaser  either  with  respect  to  the  test-bed  floor  plane 
and  to  the  moving  frame,  centered  into  the  center  of 
gravity  of  the  target  vehicle.  Fig.  13  shows  the  analogies 
with  the  docking  orbital  case. 

Using  the  inertial  reference  frame  (up  to  2  m  relative 
distance),  chaser  motions  are  described  by  the  rigid  body 
3  DOFs  dynamic  equations  with  constant  mass,  and  by  the 


a:  Phasing  (Absolute  nav) 
b:  Far  Range  RV  (Relative  nav) 
c:  Close  Range  RV  &  docking 
d:  De-Docking  and  departure 


Xi  [m] 


Fig.  17.  Error  between  desired  and  simulated  position  (m). 


Fig.  15.  Mission  profile. 


Fig.  18.  Error  between  desired  and  simulated  attitude  (deg). 
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Euler  equation  for  the  attitude: 


(14) 


where  mc  and  Ic  are  respectively  the  mass  and  the  inertia 

of  the  chaser.  F  c  and  Mc  are  the  force  (along  x,  y)  and  the 
torque  (around  z)  applied  to  the  chaser,  calculated 
through  the  Actuator  Model. 

Considering  the  relative  reference  frame,  the  chaser 
uses  relative  sensors  information  to  estimate  its  position 
and  attitude  with  respect  to  the  target  vehicle.  The 
dynamic  equations  usually  implemented  in  this  case  are 
the  Clohessy-Whiltshire  [15]  linear  equations  written  into 
local  orbital  plane  centered  on  the  CoG  of  the  target.  For 
chaser’s  maneuvers  performed  in  the  vicinity  of  the 
target,  i.e.  the  local  orbital  plane  origin,  elapsing  a  short 
time  in  comparison  to  the  orbital  period,  the  equations 
can  be  further  simplified  in  a  double  integrator  for  both  x 
and  y  directions,  that  also  reflects  the  dynamics  of  the 
vehicles  of  the  test  bed.  So  the  equations  implemented  are 
(see  Ref.  6  for  details): 


he 

mc 


(15) 


FyC 

m  c 


(16) 


a  ■  ■  IV‘T  MC 

Orel  =  (OzT-COzC  =  ~  (17) 

where  the  subscript  rel  indicates  the  relative  reference 
frame,  centered  in  the  target  CoG. 

As  we  can  see  from  the  equations  above,  the  use  of  the 
relative  system  equations  requires  the  knowledge  of  the 
target  state.  For  this  reason  all  the  tests  are  performed 
with  an  assigned  dynamics  of  the  target  that  in  this  case 
can  be  expressed  in  terms  of  forces  FT  and  torque  MT 
applied  to  the  target. 


Measurements  about  angular  rate  and  acceleration, 
detected  by  the  z-gyroscope  and  the  accelerometers  are 
integrated  by  the  INS  producing  position  and  attitude  bias 
that  will  be  reset  by  the  laser  every  40  s  (Fig.  14). 


6.  Results  of  simulation 


A  typical  mission  profile  with  fixed  target  is  shown  in 
Fig.  15,  in  which  the  initial  chaser  position  is  (0,0)  and  the 
target  is  fixed  at  coordinates  (4,4)  with  an  attitude  of  -  90°. 
The  MPM  is: 


MPM  = 


"1 

2 

3 

2 


4  0  0.06- 

0  4  0.05 

90  2  0 

-4  -4  0.08  _ 


So  the  chaser  switches  from  absolute  frame  to  relative 
reference  frame  after  130  s  at  a  relative  distance  from  the 
target  equal  to  2  m.  Fig.  1 6  compares  the  desired  path 
with  the  simulation. 


5.3.  The  Sensors  model 


Fig.  19.  Error  between  real  and  Kalman  filter  estimated  position  (m). 


From  the  calculation  of  position  and  attitude,  thanks  to 
the  equations  of  motion,  the  sensor  performance  are 
modeled  in  order  to  reproduce  the  measurement  vector 
z  as  input  to  the  IKF.  The  laser,  the  camera  and  the  radio¬ 
finder  sampling  frequencies  are  respectively  50  Hz,  10  Hz 
and  20  Hz  and  they  are  modeled  as  a  sum  of  a  zero  mean 
Gaussian  noise  with  assigned  standard  deviation  plus  a 
constant  bias  (see  Table  1). 

The  performance  of  the  IMU  [16]  takes  into  account  for 
several  types  of  errors,  like: 

•  Bias  repeatability:  it’s  constant  during  functional 
operation  but  different  at  any  power  on  of  the 
instrument. 

•  Angle/velocity  random  walk,  modeled  as  zero  mean 
Gaussian  noise  at  high  frequency,  and  it’s  due  to  the 
thermo-mechanical  noise  of  the  instrument. 

•  Bias  stability,  due  to  flicker  noise,  and  it  is  modeled  as 
a  random  walk  and  it  generates  a  drift  factor  up  to  30  s 
of  operation. 


Fig.  20.  Kalman  filter  evaluation  based  on  all  sensors  (m). 
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Fig.  17  shows  an  error  equal  to  30  mm  during  absolute 
navigation  and  a  control  outside  a  dead  band  of  5.5  mm 
during  relative  navigation,  matching  the  mission  require¬ 
ments  listed  in  Fig.  4.  During  linear  maneuver,  no  actions 
are  taken  to  control  the  attitude,  since  the  error  remains 
inside  the  dead  band  of  5°,  set  for  absolute  navigation  and 
greater  than  that  of  relative  navigation,  set  to  1°.  During 
the  rotation  maneuver,  the  error  between  desired  and 
simulated  attitude  is  of  the  order  of  4°  (Fig.  18).  The 
estimate  of  the  IKF  is  presented  in  Fig.  19. 

The  initial  error  is  due  to  overshoot  during  maneuver 
along  x  and  y  respectively. 

When  the  Failure  Detection  Identification  and  Recov¬ 
ery  modules  of  the  MMA  detect  a  difference  between 
information  from  the  different  sensors  above  a  certain 
threshold,  it  sends  an  alarm  vector  to  the  IKF  in  order  to 
ignore  that  information,  from  a  failed  sensor,  in  the 
calculation  of  the  estimated  state.  Inside  the  Information 
Kalman  Filter,  the  value  of  the  observation  noise  of  that 
sensor  will  be  noticeably  increased,  through  the  error 
covariance  matrix  R,  so  the  filter  will  exclude  the  unreli¬ 
able  information.  The  following  figures  compare  the 
estimated  state  of  the  Information  Kalman  filter  in  case 
of  nominal  IMU  and  laser  function  (Fig.  20)  and  after  laser 
failure  (Fig.  21 ).  Different  behavior  between  x  and  y  errors 


Fig.  21.  Kalman  filter  evaluation  without  laser  sensor  (m). 


Fig.  22.  Position  error  without  laser  sensor  (absolute  reference 
frame)  (m). 


is  the  result  of  thrusters  mixing  of  velocity  and  attitude 
control. 

The  failure  of  laser  during  overall  mission  produces  a 
loss  of  accuracy  in  the  estimate  of  the  state  equal  to 
12  mm,  which  is  reflected  on  the  control  as  a  loss  of  the 
final  accuracy  on  position  of  4  cm  (Fig.  22),  so  the  docking 
cannot  be  performed  according  to  most  stringent  mission 
requirements.  This  loss  in  accuracy  is  mainly  due  to  IMU 
error  drift  which  cannot  be  reset  by  the  laser  in  failure. 
However  during  relative  navigation,  the  final  estimate  of 
Kalman  filter  doesn’t  depend  on  IMU  measurements. 

7.  Conclusions 

The  GNC  algorithms  are  designed  and  developed  using 
a  simulation  environment  in  which  the  dynamic  model  of 
vehicles  including  the  sensors  was  considered,  imple¬ 
menting  the  equations  of  motion  between  two  rigid 
bodies,  one  of  which,  the  target,  has  a  known  kinematics. 
The  navigation  algorithm  works  both  in  relative  and 
absolute  coordinate  frame  and  estimates  the  distance  at 
which  the  switch  of  reference  frame  may  occur.  The 
chosen  Information  Kalman  filter  allows  the  fusion 
between  multiple  sensors  and  it  is  able  to  estimate 
position,  attitude  and  velocity  with  a  bounded  error  (i.e. 
±  3  mm  and  0.5°).  The  error  due  to  INS  integration  is  reset 
by  the  laser  measurements.  The  guidance  algorithms 
calculate  the  desired  velocity  and  position  profile  starting 
from  a  pre-defined  path,  assigned  inside  the  MPM,  and 
compare  them  with  the  navigation  output.  The  control 
algorithm  manages  the  activation  of  14  cold  thrusters  and 
the  respective  level  of  thrust.  If  the  laser  sensor  fails 
during  absolute  navigation,  the  error  on  position  is  of  the 
order  of  4  cm,  so  in  this  case  the  requirements  are  not 
accomplished.  The  final  accuracy  of  the  GNC,  in  nominal 
conditions,  ensures  the  completion  of  the  mission  with  a 
precision  of  5°  and  30  mm  up  to  2  m  from  the  target,  and 
5.5  mm  for  lower  range. 

As  a  final  remark,  even  if  only  final  experiments  on  the 
physical  rig,  with  hardware  in  the  loop,  can  confirm  the 
ability  of  the  GNC  algorithms  to  comply  with  the  design 
requirements,  the  use  of  simulations  provides  a  compre¬ 
hensive  approach,  a  method  for  assessing  and  tuning  the 
software  prototype,  before  the  complete  hardware  layout 
is  available.  Additionally,  a  description  of  the  developed 
RV&D  experimental  rig  is  here  detailed. 

Nomenclature 

oci  Acceleration  during  i-th  maneuver 

b[  Observation  noise  of  i-th  sensor 

B k  Control  input  matrix 

Fc  Force  applied  to  the  chaser 

Ft  Force  applied  to  the  target 

F(s)  Transfer  function  of  ON-OFF  signal 
FY i  Maximum  thrust  for  Y1  group 

Hk  Observation  matrix 

Iy  Required  pulse  along  y  body  axis 

IY i  Required  pulse  of  Y1  group 

k  Duration  of  transient 
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I<i  Kalman  gain  of  i-th  sensor 

I<i  Integrative  gain  of  controller 

I<D  Derivative  gain  of  controller 

KP  Proportional  gain  of  controller 

Pk  Error  covariance  matrix 

Qk  Process  noise  covariance 

Wi  Measurement  noise  covariance  of  i-th  sensor 

Tstk  Duration  of  station  keeping  maneuver 

u  Control  vector 

Vdi  Drift  velocity  during  i-th  maneuver 

XfJi  Waypoint  coordinates  of  i-th  maneuver 

x  State  vector 

xgUi,ygUi  Desired  position  in  switching  reference  frame 

Xnav,ymv  Estimated  position  in  switching  reference  frame 

Zi  Measurement  vector  of  i-th  sensor 

AT  at  Duration  of  attitude  maneuver 

A Tnn  Duration  of  linear  maneuver 

A Tcon  PID  sampling  time 

A TPWM  PWM  sampling  time 

Sj  Angular  rotation  of  j-th  maneuver 

6j  Angular  velocity  of  j-th  maneuver 

6j  Angular  acceleration  of  j-th  maneuver 

Tthr  Percentage  of  thruster  ON-time 

Acronyms 

CC  Control  Center 

MPM  Mission  Plan  matrix 

CoG  Center  of  Gravity 

PID  Proportional  Integrative  Derivative 

DOF  Degree  of  Freedom 

PWM  Pulse  Width  Modulator 

GNC  Guidance  Navigation  Control 

RV&D  Rendezvous  &  Docking 

INS  Inertial  Navigation  System 

STEPS  System  and  Technologies  for  Space  Exploration 

MMA  Mission  Management  Agent 

ZMWG  Zero  Mean  White  Gaussian 
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